package zjut.raymond.witdemo.algorithm;


import android.graphics.PointF;

import java.util.Timer;
import java.util.TimerTask;

/**
 * 平滑步行寻路器
 */

public class Router {
    public static double A = 0.5;                                               // 加速度(正)
    public static double VMax = 1.2;                                            // 速度最大值
    public static double VMin = 0.3;                                            // 初速度(速度最小值)
    public static double S0 = 100 * (Math.pow(VMax, 2) - Math.pow(VMin, 2)) / (2 * A);  // 最高速度降到最低速度的面积

    // 全局开关
    private boolean mEnable = true;

    // 移动速度(m/s)
    private double D0 = 0;      // 总距离
    private double V = 0;       // 瞬时速度
    private double D = 0;       // 瞬时距离
    private double S = 0;       // 降到最低速度的距离

    // 贝塞尔曲线平滑(尚未开发)
    // private boolean mBezier = false;

    // 点
    private PointF mStartPoint;
    private PointF mNowPoint;
    private PointF mTargetPoint;

    // 刷新频率(秒)
    private double deltaT = 0.02D;

    /**
     * 制造一个新的平滑步行寻路器
     */
    public Router() {
        new Timer().schedule(new RouterTimeTask(), 0, (long) (deltaT * 1000));
    }

    public void set(PointF targetPoint) {
        if (mNowPoint == null || !mEnable) {
            mNowPoint = targetPoint;
            V = VMin;
        } else {
            mStartPoint = new PointF(mNowPoint.x, mNowPoint.y);
            mTargetPoint = targetPoint;
            D0 = Math.hypot(mTargetPoint.x - mNowPoint.x, mTargetPoint.y - mNowPoint.y);
        }
    }

    public PointF get() {
        return mNowPoint;
    }

    /**
     * 平滑器线程
     */
    private class RouterTimeTask extends TimerTask {
        @Override
        public void run() {
            if (mNowPoint != null && mTargetPoint != null) {
                D = Math.hypot(mTargetPoint.x - mNowPoint.x, mTargetPoint.y - mNowPoint.y);
                S = 100 * (Math.pow(V, 2) - Math.pow(VMin, 2)) / (2 * A);
                if (D > 1) {
                    if (V < VMax) {
                        if (S < D) V += A * deltaT;
                        else V -= A * deltaT;
                    } else {
                        if (S0 < D) V = VMax;
                        else V -= A * deltaT;
                    }
                    double unitMove = V * deltaT * 100;
                    double ratio = unitMove / D0;
                    if (ratio <= 1) {
                        mNowPoint.x += ratio * (mTargetPoint.x - mStartPoint.x);
                        mNowPoint.y += ratio * (mTargetPoint.y - mStartPoint.y);
                    }
                }
            }
        }
    }

    /**
     * Debug use only
     *
     * @return 返回当前的运动数值
     */
    public String getMoveStatus() {
        StringBuilder sb = new StringBuilder();

        if (mStartPoint == null) sb.append("X");
        else sb.append(mStartPoint.toString());
        sb.append("->\t");
        if (mNowPoint == null) sb.append("X");
        else sb.append(mNowPoint.toString());
        sb.append(" ->\t");
        if (mStartPoint == null) sb.append("X");
        else sb.append(mStartPoint.toString());

        if (mEnable) return sb.toString();
        else return "N/A";
    }

    /**
     * Switch of this router
     *
     * @param enable switch
     */
    public void setEnable(boolean enable) {
        mEnable = enable;
    }

    /**
     * get status of this router
     *
     * @return whether this router is enabled
     */
    public boolean isEnable() {
        return mEnable;
    }

}
